package org.hypheli.testApps;

import java.text.DecimalFormat;
import java.text.NumberFormat;
import java.util.List;

import org.hh.common.KeyPoint;
import org.hh.common.MapManager;
import org.hh.locodo.Locomotion;

import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
//import com.googlecode.javacv.cpp.opencv_imgproc;

import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.graphics.Path;

public class Mobile extends Locomotion {

	private List<KeyPoint> fShape;
	private CvPoint fShapeCvPoint = null;
	private double fConvMeterToPix;
	Path fRobotPath = null;
	NumberFormat fNumberFormat;
	
	public Mobile(String name, List<KeyPoint> polygon, double encoderWheelPerimeter,
			double encoderWheelsDistanceToCenter, int encoderTickPerRotation, double convMeterToPix) {
		super(name, encoderWheelPerimeter, encoderWheelsDistanceToCenter,
				encoderTickPerRotation);
		
		fShape = polygon;
		fConvMeterToPix = convMeterToPix;
	  fNumberFormat = new DecimalFormat("0.000");

	}

	public void drawNoObstacleShape(Canvas canvas, Paint paint, IplImage probaImg, int zoomFactor){
		org.hh.common.KeyPoint pos = getPosition();
		
    if( fShapeCvPoint == null ){
    	fShapeCvPoint = new CvPoint(fShape.size());
    }
  	int i = 0;
    for(KeyPoint p : fShape){
    	
    	CvPoint r = MapManager.rotationPoint(p.x+pos.x, p.y + pos.y, pos, pos.a, fConvMeterToPix/zoomFactor);
    	fShapeCvPoint.position(i++).x(r.x())
    														 .y(r.y());
    }    	

    opencv_core.cvFillConvexPoly(probaImg, fShapeCvPoint.position(0), fShape.size(), opencv_core.CvScalar.BLACK, opencv_core.CV_AA, 0);

	}
	
	public void onDraw(Canvas canvas, Paint robotPaint){
		///////////// Draw the robot /////////////
		KeyPoint pos = getPosition();
		KeyPoint target = getTarget();
		double[] realSpeed = getRealSpeed();
		double[] speed = getSpeed();
		
    int px = (int)(pos.x * fConvMeterToPix);
    int py = (int)(pos.y * fConvMeterToPix);

    int pTargetx = (int)(target.x * fConvMeterToPix);
    int pTargety = (int)(target.y * fConvMeterToPix);

    canvas.save(); 		//================>
    canvas.translate(px, py);
    canvas.rotate((float)( -pos.a * 180.0 / Math.PI ));

    // draw robot body
  	Path robotPath = new Path();
  	boolean first = true; 
    for(KeyPoint p : fShape){
    	if( first ){
    		robotPath.moveTo(	(int)(p.x * fConvMeterToPix), 
    											(int)(p.y * fConvMeterToPix));
    		first = false;
    		continue;
    	}
    	robotPath.lineTo( (int)(p.x * fConvMeterToPix), 
    										(int)(p.y * fConvMeterToPix));
    }
    robotPaint.setColor(Color.DKGRAY);
    canvas.drawPath(robotPath, robotPaint);
    
    // draw the wheel with the real speed vector
    int x1 = (int) (fEncoderWheelsDistanceToCenter * fConvMeterToPix);
    int x2 = x1 - 2; // 2 pix width
    int y1 = (int) ((fEncoderWheelDiameter / 2.0) * fConvMeterToPix);
    int y2 = -y1; 
    
    robotPaint.setColor(Color.BLUE);
    canvas.drawRect( y1, x1, y2, x2, robotPaint);
    canvas.drawRect( -1, x1, 1, x1 + (float)(realSpeed[1]/2.), robotPaint);
    canvas.drawRect( -2, x1, -1, x1 + (float)(speed[1]/0.02), robotPaint);
    
    robotPaint.setColor(Color.GRAY);
    canvas.drawRect( y1, -x2, y2, -x1, robotPaint);
    canvas.drawRect( -1, -x2, 1, -x2 - (float)(realSpeed[0]/2.), robotPaint);
    canvas.drawRect( -2, -x2, -1, -x2 - (float)(speed[0]/0.02), robotPaint);
    
    
    // draw the robot center
    robotPaint.setColor(Color.YELLOW);
    canvas.drawCircle(0, 0, 1, robotPaint);

    canvas.restore(); //<================
    

		///////////// Draw string info and the target ///////////// 
    canvas.save(); 		//================>
  	// pos feedback
  	robotPaint.setColor(Color.WHITE);
  	
		final int connectedPosX = 672;

  	String posString = "p " + fNumberFormat.format(pos.x) + " " + fNumberFormat.format(pos.y) + " " + fNumberFormat.format(pos.a);
  	canvas.drawText(posString, connectedPosX+10, 10, robotPaint);
  	posString = "speed " + fNumberFormat.format(speed[0]) + " " + fNumberFormat.format(speed[1]);
  	canvas.drawText(posString, connectedPosX+10, 20, robotPaint);
  	posString = "reals " + fNumberFormat.format(realSpeed[0]) + " " + fNumberFormat.format(realSpeed[1]);
  	canvas.drawText(posString, connectedPosX+10, 30, robotPaint);
  	
  	
    robotPaint.setColor(Color.GREEN);
    canvas.drawCircle(pTargetx, pTargety, 3, robotPaint);
    canvas.restore(); //<================
    
	}

}
